#ifndef __COSTMAP_2D_ROS_H_
#define __COSTMAP_2D_ROS_H_

#include <ros/ros.h>
#include <XmlRpcValue.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/message_filter.h>
#include <message_filters/subscriber.h>
#include <laser_geometry/laser_geometry.h>

#include "common/layered_costmap.h"
#include "layers/static_layer.h"
#include "layers/sensors_layer.h"
#include "layers/inflation_layer.h"
#include "visual/costmap_publish.h"

namespace costmap_2d{

// 定义Costmap2DROS
class Costmap2DROS{
    public:    
        Costmap2DROS(const std::string &name, tf2_ros::Buffer& tf);
        ~Costmap2DROS();

        std::vector<std::string> parseSensorsList(ros::NodeHandle& nh);

        void mapUpdateLoop(double frequency);
        bool getRobotPose(geometry_msgs::PoseStamped& global_pose) const;


        void mapCallback(const nav_msgs::OccupancyGridConstPtr& new_map);
        void laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, string topic_name);
        void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message, string topic_name);

        LayeredCostmap* getLayeredCostmap() const;

    protected:
        LayeredCostmap* layered_costmap_;
        tf2_ros::Buffer& tf_;
        Costmap2DPublisher* publisher_;
        laser_geometry::LaserProjection projector_;
        boost::thread* map_update_thread_;

        std::string name_;
        std::string map_topic_;
        std::string global_frame_;
        std::string robot_base_frame_;
        double transform_tolerance_;
        double update_frequency_;

    private:
        ros::NodeHandle nh_;
        ros::Subscriber map_sub_;
        std::vector<boost::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_;
        std::vector<boost::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_;
        boost::shared_ptr<Layer> static_layer_;
        boost::shared_ptr<Layer> sensors_layer_;
        boost::shared_ptr<Layer> inflation_layer_;
};


}   // namespace costmap_2d

#endif